/* Vecmath.

   Copyright (C) 2001, 2002, 2003 Stefan Maierhofer.

   Written by Stefan Maierhofer <sm@cg.tuwien.ac.at>

   This file is part of Vecmath.

   Vecmath is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.

   Vecmath is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with Vecmath; if not, write to the Free Software Foundation,
   Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA. */

using System;
using NUnit.Framework;

namespace Vecmath
{

    /// <summary>
    /// Trafo3D.
    /// </summary>
    public struct Trafo3D
    {
        public static Trafo3D Identity = new Trafo3D(
            1,0,0,0,
            0,1,0,0,
            0,0,1,0,
            0,0,0,1
            );

        public Trafo3D(
            double x00, double x01, double x02, double x03,
            double x10, double x11, double x12, double x13,
            double x20, double x21, double x22, double x23,
            double x30, double x31, double x32, double x33
            )
        {
            _x = new double[16];
            _x[ 0] = x00; _x[ 1] = x01; _x[ 2] = x02; _x[ 3] = x03;
            _x[ 4] = x10; _x[ 5] = x11; _x[ 6] = x12; _x[ 7] = x13;
            _x[ 8] = x20; _x[ 9] = x21; _x[10] = x22; _x[11] = x23;
            _x[12] = x30; _x[13] = x31; _x[14] = x32; _x[15] = x33;
        }

        public Trafo3D(Rot3D r)
        {
            this = r.ToTrafo3D();
        }

        
        public Trafo3D(Trafo2D t)
        {
            _x = new double[16];
            _x[ 0] = t[0,0]; _x[ 1] = t[0,1]; _x[ 2] = 0.0; _x[ 3] = t[0,2];
            _x[ 4] = t[1,0]; _x[ 5] = t[1,1]; _x[ 6] = 0.0; _x[ 7] = t[1,2];
            _x[ 8] = 0.0; _x[ 9] = 0.0; _x[10] = 1.0; _x[11] = 0.0;
            _x[12] = t[2,0]; _x[13] = t[2,1]; _x[14] = 0.0; _x[15] = t[2,2];
        }
        
        public static implicit operator Trafo3D(Trafo3F t)
        {
            return new Trafo3D(
                t._x[ 0], t._x[ 1], t._x[ 2], t._x[ 3],
                t._x[ 4], t._x[ 5], t._x[ 6], t._x[ 7], 
                t._x[ 8], t._x[ 9], t._x[10], t._x[11], 
                t._x[12], t._x[13], t._x[14], t._x[15]
                );
        }

        public double this [int i]
        {
            get { return _x[i]; }
            set { _x[i] = value; }
        }

        public double this [int i, int j]
        {
            get { return _x[i*4+j]; }
            set { _x[i*4+j] = value; }
        }

        public Trafo3D Transposed
        {
            get
            {
                return new Trafo3D(
                    _x[0], _x[4], _x[ 8], _x[12],
                    _x[1], _x[5], _x[ 9], _x[13],
                    _x[2], _x[6], _x[10], _x[14],
                    _x[3], _x[7], _x[11], _x[15]
                    );
            }
        }

        public void Transpose()
        {
            this = this.Transposed;
        }

        public double Det
        {
            get { return Vecmath.Det4x4(_x); }
        }

        public Trafo3D Adjoint
        {
            get
            {
                return new Trafo3D(
                    Vecmath.Det3x3(
                        _x[ 5], _x[ 6], _x[ 7],
                        _x[ 9], _x[10], _x[11],
                        _x[13], _x[14], _x[15]), // 00
                    - Vecmath.Det3x3(
                        _x[ 4], _x[ 6], _x[ 7],
                        _x[ 8], _x[10], _x[11],
                        _x[12], _x[14], _x[15]), // 01
                    Vecmath.Det3x3(
                        _x[ 4], _x[ 5], _x[ 7],
                        _x[ 8], _x[ 9], _x[11],
                        _x[12], _x[13], _x[15]), // 02
                    - Vecmath.Det3x3(
                        _x[ 4], _x[ 5], _x[ 6],
                        _x[ 8], _x[ 9], _x[10],
                        _x[12], _x[13], _x[14]), // 03

                    Vecmath.Det3x3(
                        _x[ 1], _x[ 2], _x[ 3],
                        _x[ 9], _x[10], _x[11],
                        _x[13], _x[14], _x[15]), // 10
                    - Vecmath.Det3x3(
                        _x[ 0], _x[ 2], _x[ 3],
                        _x[ 8], _x[10], _x[11],
                        _x[12], _x[14], _x[15]), // 11
                    Vecmath.Det3x3(
                        _x[ 0], _x[ 1], _x[ 3],
                        _x[ 8], _x[ 9], _x[11],
                        _x[12], _x[13], _x[15]), // 12
                    - Vecmath.Det3x3(
                        _x[ 0], _x[ 1], _x[ 2],
                        _x[ 8], _x[ 9], _x[10],
                        _x[12], _x[13], _x[14]), // 13

                    Vecmath.Det3x3(
                        _x[ 1], _x[ 2], _x[ 3],
                        _x[ 5], _x[ 6], _x[ 7],
                        _x[13], _x[14], _x[15]), // 20
                    - Vecmath.Det3x3(
                        _x[ 0], _x[ 2], _x[ 3],
                        _x[ 4], _x[ 6], _x[ 7],
                        _x[12], _x[14], _x[15]), // 21
                    Vecmath.Det3x3(
                        _x[ 0], _x[ 1], _x[ 3],
                        _x[ 4], _x[ 5], _x[ 7],
                        _x[12], _x[13], _x[15]), // 22
                    - Vecmath.Det3x3(
                        _x[ 0], _x[ 1], _x[ 2],
                        _x[ 4], _x[ 5], _x[ 6],
                        _x[12], _x[13], _x[14]), // 23

                    Vecmath.Det3x3(
                        _x[ 1], _x[ 2], _x[ 3],
                        _x[ 5], _x[ 6], _x[ 7],
                        _x[ 9], _x[10], _x[11]), // 24
                    - Vecmath.Det3x3(
                        _x[ 0], _x[ 2], _x[ 3],
                        _x[ 4], _x[ 6], _x[ 7],
                        _x[ 8], _x[10], _x[11]), // 25
                    Vecmath.Det3x3(
                        _x[ 0], _x[ 1], _x[ 3],
                        _x[ 4], _x[ 5], _x[ 7],
                        _x[ 8], _x[ 9], _x[11]), // 26
                    - Vecmath.Det3x3(
                        _x[ 0], _x[ 1], _x[ 2],
                        _x[ 4], _x[ 5], _x[ 6],
                        _x[ 8], _x[ 9], _x[10])  // 27
                    );
            }
        }

        public Trafo3D Inverse
        {
            get
            {
                double d = Det;
                if (d == 0) throw new ArithmeticException("Singular Matrix");
                d = 1.0 / d;
                return Adjoint.Transposed * d;
            }
        }

        public void Invert()
        {
            this = Inverse;
        }

        public static Trafo3D operator+(Trafo3D t, double f)
        {
            return new Trafo3D(
                t._x[ 0] + f, t._x[ 1] + f, t._x[ 2] + f, t._x[ 3] + f,
                t._x[ 4] + f, t._x[ 5] + f, t._x[ 6] + f, t._x[ 7] + f,
                t._x[ 8] + f, t._x[ 9] + f, t._x[10] + f, t._x[11] + f,
                t._x[12] + f, t._x[13] + f, t._x[14] + f, t._x[15] + f
                );
        }

        public static Trafo3D operator-(Trafo3D t, double f)
        {
            return new Trafo3D(
                t._x[ 0] - f, t._x[ 1] - f, t._x[ 2] - f, t._x[ 3] - f,
                t._x[ 4] - f, t._x[ 5] - f, t._x[ 6] - f, t._x[ 7] - f,
                t._x[ 8] - f, t._x[ 9] - f, t._x[10] - f, t._x[11] - f,
                t._x[12] - f, t._x[13] - f, t._x[14] - f, t._x[15] - f
                );
        }

        public static Trafo3D operator*(Trafo3D t, double f)
        {
            return new Trafo3D(
                t._x[ 0] * f, t._x[ 1] * f, t._x[ 2] * f, t._x[ 3] * f,
                t._x[ 4] * f, t._x[ 5] * f, t._x[ 6] * f, t._x[ 7] * f,
                t._x[ 8] * f, t._x[ 9] * f, t._x[10] * f, t._x[11] * f,
                t._x[12] * f, t._x[13] * f, t._x[14] * f, t._x[15] * f
                );
        }

        public static Trafo3D operator/(Trafo3D t, double f)
        {
            return new Trafo3D(
                t._x[ 0] / f, t._x[ 1] / f, t._x[ 2] / f, t._x[ 3] / f,
                t._x[ 4] / f, t._x[ 5] / f, t._x[ 6] / f, t._x[ 7] / f,
                t._x[ 8] / f, t._x[ 9] / f, t._x[10] / f, t._x[11] / f,
                t._x[12] / f, t._x[13] / f, t._x[14] / f, t._x[15] / f
                );
        }

        public static Pnt3D operator*(Pnt3D p, Trafo3D t)
        {
            double f = 1.0 / (p[0]*t[0,3]+p[1]*t[1,3]+p[2]*t[2,3]+t[3,3]);
            return new Pnt3D(
                (p[0] * t[0,0] + p[1] * t[1,0] + p[2] * t[2,0] + t[3,0]) * f,
                (p[0] * t[0,1] + p[1] * t[1,1] + p[2] * t[2,1] + t[3,1]) * f,
                (p[0] * t[0,2] + p[1] * t[1,2] + p[2] * t[2,2] + t[3,2]) * f
                );
        }

        public static Vec3D operator*(Vec3D v, Trafo3D t)
        {
            return new Vec3D(
                v._x[0] * t._x[0] + v._x[1] * t._x[4] + v._x[2] * t._x[ 8],
                v._x[0] * t._x[1] + v._x[1] * t._x[5] + v._x[2] * t._x[ 9],
                v._x[0] * t._x[2] + v._x[1] * t._x[6] + v._x[2] * t._x[10]
                );
        }

        public static Ray3D operator*(Ray3D r, Trafo3D t)
        {
            return new Ray3D(r.p * t, r.v * t);
        }

        public static Box3D operator*(Box3D b, Trafo3D t)
        {
            Box3D result = Box3D.Empty;
            result += b.LLL * t;
            result += b.LLU * t;
            result += b.LUL * t;
            result += b.LUU * t;
            result += b.ULL * t;
            result += b.ULU * t;
            result += b.UUL * t;
            result += b.UUU * t;
            return result;
        }

        public static Trafo3D operator*(Trafo3D a, Trafo3D b)
        {
            return new Trafo3D(

                a._x[ 0]*b._x[ 0]+a._x[ 1]*b._x[ 4]+a._x[ 2]*b._x[ 8]+a._x[ 3]*b._x[12],
                a._x[ 0]*b._x[ 1]+a._x[ 1]*b._x[ 5]+a._x[ 2]*b._x[ 9]+a._x[ 3]*b._x[13],
                a._x[ 0]*b._x[ 2]+a._x[ 1]*b._x[ 6]+a._x[ 2]*b._x[10]+a._x[ 3]*b._x[14],
                a._x[ 0]*b._x[ 3]+a._x[ 1]*b._x[ 7]+a._x[ 2]*b._x[11]+a._x[ 3]*b._x[15],

                a._x[ 4]*b._x[ 0]+a._x[ 5]*b._x[ 4]+a._x[ 6]*b._x[ 8]+a._x[ 7]*b._x[12],
                a._x[ 4]*b._x[ 1]+a._x[ 5]*b._x[ 5]+a._x[ 6]*b._x[ 9]+a._x[ 7]*b._x[13],
                a._x[ 4]*b._x[ 2]+a._x[ 5]*b._x[ 6]+a._x[ 6]*b._x[10]+a._x[ 7]*b._x[14],
                a._x[ 4]*b._x[ 3]+a._x[ 5]*b._x[ 7]+a._x[ 6]*b._x[11]+a._x[ 7]*b._x[15],

                a._x[ 8]*b._x[ 0]+a._x[ 9]*b._x[ 4]+a._x[10]*b._x[ 8]+a._x[11]*b._x[12],
                a._x[ 8]*b._x[ 1]+a._x[ 9]*b._x[ 5]+a._x[10]*b._x[ 9]+a._x[11]*b._x[13],
                a._x[ 8]*b._x[ 2]+a._x[ 9]*b._x[ 6]+a._x[10]*b._x[10]+a._x[11]*b._x[14],
                a._x[ 8]*b._x[ 3]+a._x[ 9]*b._x[ 7]+a._x[10]*b._x[11]+a._x[11]*b._x[15],

                a._x[12]*b._x[ 0]+a._x[13]*b._x[ 4]+a._x[14]*b._x[ 8]+a._x[15]*b._x[12],
                a._x[12]*b._x[ 1]+a._x[13]*b._x[ 5]+a._x[14]*b._x[ 9]+a._x[15]*b._x[13],
                a._x[12]*b._x[ 2]+a._x[13]*b._x[ 6]+a._x[14]*b._x[10]+a._x[15]*b._x[14],
                a._x[12]*b._x[ 3]+a._x[13]*b._x[ 7]+a._x[14]*b._x[11]+a._x[15]*b._x[15]

                );
        }

        internal double [] _x;

        public double [] Array { get { return _x; } }

    }


    [TestFixture]
    public class Trafo3DTest
    {
        [Test]
        public void Inverse()
        {
            Trafo3D t =
                (Trafo3D)new Scale3D(1,2,3) *
                (Trafo3D)new Shift3D(4,5,6) *
                (Trafo3D)new Rot3D(2.345, new Vec3D(10,5,2).Normalized);
            Trafo3D inv = t.Inverse;

            Assert.IsTrue(Vecmath.ApproxEquals(t * inv, Trafo3D.Identity));
        }
    }
   
 
}
